#include "assert.h"
#include "opencv2/opencv.hpp"

using namespace std;
using namespace cv;

// fit 3D points using algorithm from
// "Least-Squares Fitting of Two 3-D Point Sets"


class PointFitter
{
public:
    //PointFitter();
    static void fit(
        vector<Point3f> pnts0,
        vector<Point3f> pnts1,
        Mat& R, Mat& T
        );
private:
    static Point3f Centeroid( vector<Point3f> pnts );
    static void    Minus( vector<Point3f> pnts, Point3f p, vector<Point3f>& output );
    static Mat     PtoM( Point3f );
    static void    HMatrix( vector<Point3f>, vector<Point3f>, Mat& );
    static void    FindRotation( Mat H, Mat& R  );
};
